#include <stdio.h>    /* Standard input/output definitions */
#include <stdint.h>   /* Standard types */
#include <stdlib.h>   /* calloc, strtol */
#include <string.h>   /* strcpy */
#include <unistd.h>   /* UNIX standard function definitions */
#include <fcntl.h>    /* File control definitions */
#include <errno.h>    /* Error number definitions */
#include <termios.h>  /* POSIX terminal control definitions */
#include <sys/ioctl.h>

#include "roombalib.h"

#define straight	0x8000

#define START	128
#define CONTROL	130
#define DRIVE	137
#define SONG	140
#define PLAY	141
#define MOTORS	138
#define LEDS	139
#define SENSOR	142

int roombadebug = 0;

// internal use only
int roomba_init_serialport( const char* serialport, speed_t baud );

Roomba* roomba_init( const char* portpath, speed_t baud ) {
    if( baud == 0 )
	baud = B115200;
    int fd = roomba_init_serialport( portpath, baud );

    if( fd == -1 )
	return NULL;
    uint8_t cmd[1];
    cmd[0] = START;
    int n = write(fd, cmd, 1);
    if( n!=1 ) {
        perror("open_port: Unable to write to port ");
        return NULL;
    }
    roomba_delay(COMMANDPAUSE_MILLIS);
    cmd[0] = CONTROL;
    n = write(fd, cmd, 1);
    if( n!=1 ) {
        perror("open_port: Unable to write to port ");
        return NULL;
    }
    roomba_delay(COMMANDPAUSE_MILLIS);
    Roomba* roomba = (Roomba*)calloc( 1, sizeof(Roomba) );
    roomba->fd = fd;
    strcpy(roomba->portpath, portpath);
    roomba->velocity = DEFAULT_VELOCITY;
    return roomba;
}

void roomba_free( Roomba* roomba ){
	if( roomba!= NULL ) {
		if( roomba->fd ) roomba_close( roomba );
			free( roomba );
	}
}

const char* roomba_get_portpath( Roomba* roomba ){
	return roomba->portpath;
}

void roomba_close( Roomba* roomba ){
	close( roomba->fd );
	roomba->fd = 0;
}

// is this Roomba pointer valid (but not necc connected)
int roomba_valid( Roomba* roomba ){
	return (roomba!=NULL && roomba->fd != 0);
}

void roomba_set_velocity( Roomba* roomba, int vel ){
	roomba->velocity = vel;
}

int roomba_get_velocity( Roomba* roomba ){
	return roomba->velocity;
}

// send an arbitrary length roomba command
int roomba_send( Roomba* roomba, const uint8_t* cmd, int len ){
	int n = write( roomba->fd, cmd, len);
	if( n!=len )
		perror("roomba_send: couldn't write to roomba");
	return (n!=len); // indicate error, can usually ignore
}

// Move Roomba with low-level DRIVE command
void roomba_drive( Roomba* roomba, int velocity, int radius ){
	uint8_t vhi = velocity >> 8;
	uint8_t vlo = velocity & 0xff;
	uint8_t rhi = radius   >> 8;
	uint8_t rlo = radius   & 0xff;
	if(roombadebug) 
		fprintf(stderr,"roomba_drive: %.2hhx %.2hhx %.2hhx %.2hhx\n", vhi, vlo, rhi, rlo);
	uint8_t cmd[] = { DRIVE, vhi,vlo, rhi,rlo };
	int n = write(roomba->fd, cmd, 5);
	if( n!=5 )
		perror("roomba_drive: couldn't write to roomba");
}

void roomba_stop( Roomba* roomba ){
	roomba_drive( roomba, 0, 0 );
}

void roomba_forward( Roomba* roomba ){
	roomba_drive( roomba, roomba->velocity, straight );
}
void roomba_forward_at( Roomba* roomba, int velocity ){
	roomba_drive( roomba, velocity, straight );
}

void roomba_backward( Roomba* roomba ){
	roomba_drive( roomba, -roomba->velocity, straight );
}
void roomba_backward_at( Roomba* roomba, int velocity ){
	roomba_drive( roomba, -velocity, straight );
}

void roomba_spinleft( Roomba* roomba ){
	roomba_drive( roomba, roomba->velocity, 1 );
}
void roomba_spinleft_at( Roomba* roomba, int velocity ){
	roomba_drive( roomba, velocity, 1 );
}

void roomba_spinright( Roomba* roomba ){
	roomba_drive( roomba, roomba->velocity,  -1 );
}
void roomba_spinright_at( Roomba* roomba, int velocity ){
	roomba_drive( roomba, velocity,  -1 );
}

void roomba_play_note( Roomba* roomba, uint8_t note, uint8_t duration ){
	uint8_t cmd[] = { SONG, 15, 1, note, duration, PLAY, 15 };
	int n = write( roomba->fd, cmd, 7);
	if( n!=7 )
		perror("roomba_play_note: couldn't write to roomba");
}

// Turns on/off the non-drive motors (main brush, vacuum, sidebrush).
void roomba_set_motors( Roomba* roomba, uint8_t mainbrush, uint8_t vacuum, uint8_t sidebrush){
	uint8_t cmd[] = { MOTORS, ((mainbrush?0x04:0)|(vacuum?0x02:0)|(sidebrush?0x01:0))};
	int n = write( roomba->fd, cmd, 2);
	if( n!=2 )
		perror("roomba_set_motors: couldn't write to roomba");
}

// Turns on/off the various LEDs.
void roomba_set_leds( Roomba* roomba, uint8_t status_green, uint8_t status_red, uint8_t spot, uint8_t clean, uint8_t max, uint8_t dirt, uint8_t power_color, uint8_t power_intensity ){
	uint8_t v = (status_green?0x20:0) | (status_red?0x10:0) | (spot?0x08:0) | (clean?0x04:0) | (max?0x02:0) | (dirt?0x01:0);
	uint8_t cmd[] = { LEDS, v, power_color, power_intensity };
	int n = write( roomba->fd, cmd, 4);
	if( n!=4 )
		perror("roomba_set_leds: couldn't write to roomba");
}

// Turn all vacuum motors on or off according to state
void roomba_vacuum( Roomba* roomba, uint8_t state ){
	roomba_set_motors( roomba, state,state,state);
}

int roomba_read_sensors( Roomba* roomba ){
	uint8_t cmd[] = { SENSOR, 0 };
	int n = write( roomba->fd, cmd, 2);
	roomba_delay(COMMANDPAUSE_MILLIS);	//hmm, why isn't VMIN & VTIME working?
	n = read( roomba->fd, roomba->sensor_bytes, 26);
	if( n!=26 ) {
		if(roombadebug)
			fprintf(stderr,"roomba_read_sensors: not enough read (n=%d)\n",n);
		return -1;
	}
	return 0;
}

void roomba_print_raw_sensors( Roomba* roomba ){
	uint8_t* sb = roomba->sensor_bytes;
	int i;
	for(i=0;i<26;i++) {
		printf("%.2hhx ",sb[i]);
	}
	printf("\n");
}

void roomba_print_sensors( Roomba* roomba ){
	uint8_t* sb = roomba->sensor_bytes;
	printf("bump: %x %x\n", bump_left(sb[0]), bump_right(sb[0]));
	printf("wheeldrop: %x %x %x\n", wheeldrop_left(sb[0]), wheeldrop_caster(sb[0]), wheeldrop_right(sb[0]));
	printf("wall: %x\n", sb[1]);
	printf("cliff: %x %x %x %x\n", sb[2],sb[3],sb[4],sb[5] );
	printf("virtual_wall: %x\n", sb[6]);
	printf("motor_overcurrents: %x %x %x %x %x\n", motorover_driveleft(sb[7]), motorover_driveright(sb[7]), motorover_mainbrush(sb[7]), motorover_sidebrush(sb[7]),  motorover_vacuum(sb[7]));
	printf("dirt: %x %x\n", sb[8],sb[9]);
	printf("remote_opcode: %.2hhx\n", sb[10]);
	printf("buttons: %.2hhx\n", sb[11]);
	printf("distance: %.4x\n",    (sb[12]<<8) | sb[13] );
	printf("angle: %.4x\n",       (sb[14]<<8) | sb[15] );
	printf("charging_state: %.2hhx\n", sb[16]);
	printf("voltage: %d\n",     (sb[17]<<8) | sb[18] );
	printf("current: %d\n",     ((int8_t)sb[19]<<8) | sb[20] );
	printf("temperature: %d\n",    sb[21]);
	printf("charge: %d\n",      (sb[22]<<8) | sb[23] );
	printf("capacity: %d\n",    (sb[24]<<8) | sb[25] );
}

// 100,000 us == 100 ms == 0.1s
void roomba_delay( int millisecs ){
	usleep( millisecs * 1000 );
}

// private
// returns valid fd, or -1 on error
int roomba_init_serialport( const char* serialport, speed_t baud ){
	struct termios toptions;
	int fd;
	if(roombadebug)
		fprintf(stderr,"roomba_init_serialport: opening port %s\n",serialport);
	fd = open( serialport, O_RDWR | O_NOCTTY | O_NDELAY );
	if (fd == -1)  {	// Could not open the port.
		perror("roomba_init_serialport: Unable to open port ");
		return -1;
	}
	if (tcgetattr(fd, &toptions) < 0) {
		perror("roomba_init_serialport: Couldn't get term attributes");
		return -1;
	}
	cfsetispeed(&toptions, baud);
	cfsetospeed(&toptions, baud);
	// 8N1
	toptions.c_cflag &= ~PARENB;
	toptions.c_cflag &= ~CSTOPB;
	toptions.c_cflag &= ~CSIZE;
	toptions.c_cflag |= CS8;
	// no flow control
	toptions.c_cflag &= ~CRTSCTS;
	toptions.c_cflag    |= CREAD | CLOCAL;					// turn on READ & ignore ctrl lines
	toptions.c_iflag    &= ~(IXON | IXOFF | IXANY);			// turn off s/w flow ctrl
	toptions.c_lflag    &= ~(ICANON | ECHO | ECHOE | ISIG);	// make raw
	toptions.c_oflag    &= ~OPOST;							// make raw
	toptions.c_cc[VMIN]  = 26;
	toptions.c_cc[VTIME] = 2;								// FIXME: not sure about this
	if( tcsetattr(fd, TCSANOW, &toptions) < 0) {
		perror("roomba_init_serialport: Couldn't set term attributes");
		return -1;
	}
	return fd;
}
